#!/bin/python3

import rospy
from ocs2_msgs.msg import mpc_target_trajectories,mpc_observation,mpc_state,mpc_input

observation = mpc_observation()
def observation_callback(msg):
    global observation
    observation = msg

def get_user_input():
    input_str = input("输入目标（角度1 角度2 角度3 位置 时间）")
    input_list = input_str.split(" ")
    if len(input_list) != 5:
        print("输入正确格式")
        return None
    
    # if input_list[0]
    theta1 = float(input_list[0])
    theta2 = float(input_list[1])
    theta3 = float(input_list[2])
    pos = float(input_list[3])
    time = float(input_list[4])
    if time<=0:
        print("时间不对")
        return None

    if observation.time == 0:
        print("未启动")
        return None
    target_time = observation.time + time
    old_state = observation.state
    target_state = mpc_state([pos, theta1,theta2,theta3,0,0])
    target_input = mpc_input([0])
    target_trajectory = mpc_target_trajectories([observation.time,target_time],
                                                 [old_state,target_state],
                                                 [target_input,target_input])
    return target_trajectory

if __name__=="__main__":
    rospy.init_node("target_publisher")
    target_pub=rospy.Publisher("cartpole_mpc_target",mpc_target_trajectories,queue_size=1)
    observation_sub = rospy.Subscriber("/cartpole/mpc_observation", mpc_observation, observation_callback)
    
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        try:
            user_input_trajectory = get_user_input()
            if user_input_trajectory is not None:
                
                # print(user_input_trajectory)
                target_pub.publish(user_input_trajectory)
        except rospy.ROSInterruptException:
            break
            
        